/*
 * To change this template, choose Tools + Templates
 * and open the template in the editor.
 */
package dynamixel;

import java.io.Serializable;
import java.util.logging.Level;
import java.util.logging.Logger;
import jssc.SerialPort;
import jssc.SerialPortException;

/**
 *
 * @author Asko
 */
public class Actuator implements Serializable{

    private int engineId;
    HostActuator host;
    int[] lastMsg = new int[1024];
    public static int[] head = {0xff, 0xff};
    public static int[] dualAddresses = {0x00, 0x06, 0x08, 0x0e, 0x1e, 0x20, 0x22,
        0x24, 0x26, 0x28, 0x30, 0x44, 0x47};
    static boolean readAccess = true;

    public Actuator(HostActuator host, int engineId) {
        this.engineId = engineId;
        this.host = host;
    }

    private int[] generatePackage(int messageType, int id, int... params) {
        int[] res = new int[6 + params.length];
        res[0] = res[1] = 0xff;
        res[2] = id;
        res[3] = 2 + params.length;
        res[4] = messageType;
        for (int i = 0; i < params.length; i++) {
            res[i + 5] = params[i];
        }
        res[res.length - 1] = getCheckSum(res);
        return res;
    }

    private int[] generatePackage(int messageType, int id) {
        int[] res = new int[6];
        res[0] = res[1] = 0xff;
        res[2] = id;
        res[3] = 2;
        res[4] = messageType;
        res[5] = getCheckSum(res);
        return res;
    }

    private int[] generatePackage(int messageType, int id, short addrs,
            int... params) {
        int[] pars = new int[params.length + 1];
        pars[0] = addrs;
        System.arraycopy(params, 0, pars, 1, params.length);
        return generatePackage(messageType, id, pars);
    }

    public static int getCheckSum(int[] buf) {
        int checkSum = 0;
        if (buf.length > 2) {
            for (int i = 2; i < buf.length - 1; i++) {
                checkSum += buf[i];
            }
        }
        return (~checkSum) & 0xff;
    }

    private int getCheckSum(int messageType, int id, int addr, int... params) {
        int res = 0x0;
        if (messageType > 0 && messageType < 8) {
            switch (messageType) {
                case HostActuator.PING: {
                    res = id + 2 + HostActuator.PING;
                }
                break;
                case HostActuator.READ_DATA: {
                    res = id + 4 + HostActuator.READ_DATA + addr + params[0];
                }
                break;
                case HostActuator.WRITE_DATA: {
                    res = id + params.length + 3 + HostActuator.WRITE_DATA
                            + addr;
                    for (int x : params) {
                        res += x;
                    }
                }
                break;
                case HostActuator.REG_WRITE: {
                    res = id + params.length + 3 + HostActuator.REG_WRITE
                            + addr;
                    for (int x : params) {
                        res += x;
                    }
                }
                break;
                case HostActuator.ACTION: {
                    res = id + 2 + HostActuator.ACTION;
                }
                break;
                case HostActuator.RESET: {
                    res = id + 2 + HostActuator.RESET;
                }
                break;
                case HostActuator.SYNC_WRITE: {
                }
                break;
            }
        }
        return (~res) & 0xff;
    }

    private byte[] getStatusPackage() {
        byte res[] = null;
        return res;
    }

    private boolean isDual(int address) {
        boolean isdual = false;
        for (int x : dualAddresses) {
            if (x == address) {
                isdual = true;
            }
        }
        return isdual;
    }

    private int getHigh(int val) {
        return val >> 8 & 0xff;
    }

    private int getLow(int val) {
        return val & 0xff;
    }

    public void setGoalPosition(int goal) {
        host.sendPacket(generatePackage(HostActuator.WRITE_DATA, engineId,
                HostActuator.GOAL_POSITION_L, getLow(goal), getHigh(goal)));
    }

    public void setWheelMode(boolean wheelMode) {
        host.sendPacket(generatePackage(HostActuator.WRITE_DATA, engineId,
                HostActuator.CW_ANGLE_LIMIT_L, 0x00, 0x00));
        host.sendPacket(generatePackage(HostActuator.WRITE_DATA, engineId,
                HostActuator.CCW_ANGLE_LIMIT_L, wheelMode ? 0 : 0xff,
                wheelMode ? 0 : 0x0f));
    }

    public void turnOnLed(boolean on) {
        host.sendPacket(generatePackage(HostActuator.WRITE_DATA, engineId,
                HostActuator.LED, on ? 1 : 0));
    }

    public void setSpeed(int speed, boolean clockWise) {
        host.sendPacket(generatePackage(HostActuator.WRITE_DATA, engineId,
                HostActuator.MOVING_SPEED_L, getLow(speed), getHigh(speed)
                | (clockWise ? 0x00 : 0x04)));
    }

    public void setPID(int p, int i, int d) {
        host.sendPacket(generatePackage(HostActuator.WRITE_DATA, engineId,
                HostActuator.D_GAIN, d));
        host.sendPacket(generatePackage(HostActuator.WRITE_DATA, engineId,
                HostActuator.I_GAIN, i));
        host.sendPacket(generatePackage(HostActuator.WRITE_DATA, engineId,
                HostActuator.P_GAIN, p));
    }

    public void setTorqueLimit(int limit) {
        if (limit < 1024) {
            host.sendPacket(generatePackage(HostActuator.WRITE_DATA, engineId,
                    HostActuator.TORQUE_LIMIT_L, getLow(limit), getHigh(limit)));
        } else {
            System.out.println("Torque limit out of range!   Engine id : "
                    + "" + engineId);
        }
    }

    public void setTorqueEnable(boolean enable) {
        host.sendPacket(generatePackage(HostActuator.WRITE_DATA, engineId,
                HostActuator.TORQUE_ENABLE, enable ? 1 : 0));
    }

    public int getCurPos() {
        return readParam(HostActuator.PRESENT_POSITION_L, 2);
    }

    public int getCurTorque() {
        return readParam(HostActuator.PRESENT_LOAD_L, 2);
    }

    public int getEngineId() {
        return engineId;
    }

    public boolean isMoving() {
        int read_val = readParam(HostActuator.MOVING, 1);
        if (read_val == -1) {
        //    System.out.println("isMoving read shit!!!");
            return true;
        }
     //   System.out.println("isMoving val: "+read_val);
        return read_val == 1;
    }

    public synchronized int readParam(int addrs, int cnt) {
        while (!readAccess) {
            try {
                Thread.sleep(5);
            } catch (InterruptedException ex) {
                Logger.getLogger(Actuator.class.getName()).log(Level.SEVERE, null, ex);
            }
        }
        readAccess = false;
        host.flushBytesCnt();
     //   System.out.println("Before send req");

        host.sendPacket(generatePackage(HostActuator.READ_DATA, engineId,
                addrs, cnt));
      //  System.out.println("Before while");
        int timeout = 50;
        while ((host.getMsgCnt() < cnt + 5) && --timeout > 0) {
            try {

                Thread.sleep(5);
            } catch (InterruptedException ex) {
                Logger.getLogger(Actuator.class.getName()).log(Level.SEVERE, null, ex);
            }
        }
       // System.out.println("After while");

        readAccess = true;

        if (timeout == 0) {
            return -1;
        }

        if (!host.isErrorInLastMsg() && host.getLastMesID() == engineId) {
            int val = 0;
            if (host.getMsgCnt() >= cnt + 5) {
                System.arraycopy(host.getLastMessage(), 0, lastMsg, 0, cnt + 5);
                for (int i = 0; i < cnt; i++) {
                    val |= lastMsg[i + 5] << 8 * i;
                }
            } else {
                return -1;
            }
            return val;

        } else {
            return -1;
        }
    }

    public void setBaudRate(int baudRate) {
        int[] ar = {};
        ar = generatePackage(HostActuator.WRITE_DATA, engineId,
                HostActuator.BAUD_RATE, baudRate);
        for (int i = 0; i < ar.length; i++) {
            System.out.printf("%x", ar[i]);
            System.out.print("  ");
        }
        System.out.println("");
        host.sendPacket(ar);
    }

    public int getBaudRate() {
        return readParam(HostActuator.BAUD_RATE, 1);
    }

    public static void main(String[] args) throws SerialPortException {

        HostActuator hos = new HostActuator("COM23", 57142);

        Actuator a1 = new Actuator(hos, 1);
        /* Actuator a2 = new Actuator(hos, 3);
         Actuator a3 = new Actuator(hos, 4);
         int b1 =  a1.getBaudRate();
         int b2 =  a2.getBaudRate();*/
        int b1 = a1.getBaudRate();

        System.out.println("B1 = " + b1);


//       SerialPort port = new SerialPort("COM7");
//        port.openPort();
//
//        port.setParams(7843, 8, 1, 0);
//
//        int[] ar = new int[8];
//        ar[0] = 0xff;
//        ar[1] = 0xff;
//        ar[2] = 2;
//        ar[3] = 4;
//        ar[4] = HostActuator.WRITE_DATA;
//        ar[5] = HostActuator.BAUD_RATE;
//        ar[6] = 0x1;
//        ar[7] = 0xdd;
//        ar[8] = 0;
//
//        port.writeIntArray(ar);
        /*for (int i = 0; i < 20; i++) {
         //Tools.playDrum(Drum.d39_Hand_Clap, 127, 2000);
         Tools.playNote(Note.p99_8_Re_Diese, Note.i98_FX_3_crystal, 127, 1000);
         try {
         Thread.sleep(500);
         } catch (InterruptedException ex) {
         Logger.getLogger(Actuator.class.getName()).log(Level.SEVERE, null, ex);
         }
         }
         port.closePort();*/

    }
}
